卡尔曼方程参考

我们把卡尔曼滤波器方程列在这里，供你随时参考。请继续 :)

变量定义

\mathbf{\hat{x}} - 状态向量

\mathbf{F} - 状态转移矩阵

\mathbf{P} - 误差协方差矩阵

\mathbf{Q} - 过程噪声协方差矩阵

\mathbf{R} - 测量噪音协方差矩阵

\mathbf{S} - 用于计算卡尔曼增益的中间矩阵

\mathbf{K} - 卡尔曼增益

\mathbf{\tilde{y}} - 预测状态和测量状态之差

\mathbf{z} - 测量向量（激光雷达数据、雷达数据等）

\mathbf{I} - 标识矩阵

预测步骤方程

预测态矢量和误差协方差矩阵

\mathbf{\hat{x} {k|k-1}} = \mathbf{F {k}} \mathbf{\hat{x}_{k-1|k-1}}

\mathbf{P_{k|k-1}} = \mathbf{F_{k}} \mathbf{P_{k-1|k-1}} \mathbf{F_{k}^T} + \mathbf{Q_{k}}

更新步骤方程

卡尔曼增益

\mathbf{S_{k}} = \mathbf{H_{k}} \mathbf{P_{k|k-1}} \mathbf{H_{k}^T} + \mathbf{R_{k}}

\mathbf{K_{k}} = \mathbf{P_{k|k-1}} \mathbf{H_{k}^T} \mathbf{S_{k}}^{-1}

更新状态向量和错误协调矩阵

\mathbf{\tilde{y_{k}}} = \mathbf{z_{k}} - \mathbf{H_{k}} \mathbf{\hat{x}_{k|k-1}}

\mathbf{\hat{x} {k|k}} = \mathbf{\hat{x} {k|k-1}} +\mathbf{ K_{k}} \mathbf{\tilde{y_{k}}}

\mathbf{P_{k|k}} = (\mathbf{I} - \mathbf{ K_{k}} \mathbf{H_{k}}) \mathbf{P_{k|k-1}}